package aStar;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.Set;

public class AStar {
	
	private static final double TILE_COST = 2;
	private static final double HEIGHT_COST = 8;
	private static final double DIRECTION_CHANGE_COST_RAIL = 25;
	private static final double DIRECTION_CHANGE_COST_ROAD = 4;
	
	private static final PathVector[] POSIBLE_DIRECTIONS = {new PathVector(1,0,0),
															new PathVector(0,1,0),
															new PathVector(-1,0,0),
															new PathVector(0,-1,0)};
	
	private Set<Tile> openTiles = new HashSet<Tile>();
	private Set<Tile> closedTiles = new HashSet<Tile>();
	
	private Tile origin;
	private Tile destination;
	private Field field;
	
	private Tile processingTile;
	private Tile currentTile;
	private PathVector changeVector;
	private PathVector changeVectorParrent;
	private PathType pathType = PathType.ROAD;
	
	private long lastTime = 0;
	
	public AStar(){
		origin = new Tile();
		destination = new Tile();
	}
	
	public void setOrigin(Tile tile) {
		origin = tile;
		origin.setParent(origin);
	}
	
	public void setDestination(Tile tile) {
		destination = tile;
	}
	
	public void setPathType(PathType pathType) {

		this.pathType = pathType;
	}

	public void setField(Field field) {
		this.field = field;
	}
	
	public Tile getOrigin() {
		return origin;
	}
	
	public Tile getDestination() {
		return destination;
	}

	public long getLastTime() {
		return lastTime;
	}
	
	public ArrayList<Tile> findPath(){
		
		lastTime = System.currentTimeMillis(); 
		
		//make sure there are no previous tiles in the lists
		openTiles.clear();
		closedTiles.clear();
		
		// Get stuff straight with origin
		origin.G = 0;
		origin.H = calcManhattan(origin);
		origin.calcF();
		
		//add origin to the open list
		currentTile = origin;
		openTiles.add(currentTile);
		
		//loop until we are at the target
		while(!openTiles.isEmpty()) {
			
			//find the tile with the lowest F
			currentTile = findMinF();
			
			// move the tile to the closedTiles list
			closedTiles.add(currentTile);
			openTiles.remove(currentTile);
			
			// Stop if the current tile is the destination
			if (currentTile == destination)
				break;
			
			//Check the surrounding tiles
			for(PathVector vector : POSIBLE_DIRECTIONS) {
				int x = currentTile.tileX + vector.getdx();
				int y = currentTile.tileY + vector.getdy();
				
				//Does the tile accualy exist?
				if (field.tileExists(x, y)) {
					//Get the tile
					processingTile = field.getTile(x, y);
					//process the tile id its walkable and not a parent
					if(processingTile.isWalkable() && !(processingTile == currentTile.getParent())) {
						processTile(vector);
					}
				}
			}
		}
		ArrayList<Tile> path;
		
		//Check if the pathfinding was exited because of an empty list
		if (!openTiles.isEmpty()) {
			//Reconstruct the path
			path = findPathVector();
		} else {
			System.out.println("NO VIABLE PATH");
			System.exit(0);
			return null;
		}
		
		//How long did it take ?
		lastTime = System.currentTimeMillis() - lastTime;
		System.out.println("Exiting pathfinder -TIME:"+lastTime+" ms-");
		
		//Finish the pathfinder
		return path;
		
	}

	private ArrayList<Tile> findPathVector() {
		ArrayList<Tile> path = new ArrayList<Tile>();
		currentTile = destination;
		path.add(destination);
		path.add(destination.getParent());
		int it = 0;
		do {			
			currentTile = currentTile.getParent();
			path.add(currentTile.getParent());
			it++;
		} while(currentTile != (origin));
		
		return path;
	}

	private void processTile(PathVector vector) {
		//Calculate the penalty due to direction change
		double directionChangePenalty = 0;
		//Create pathvector form parent to current 
		changeVector = new PathVector(currentTile.getParent(), currentTile);
		changeVectorParrent =  new PathVector(currentTile.getParent().getParent(), currentTile.getParent());
		
		switch (pathType) {
		case ROAD:
			if (!vector.equalDirection(changeVector))
				directionChangePenalty = DIRECTION_CHANGE_COST_ROAD;
			break;

		case RAIL:
			//TODO: RAILDiRECTION CHANGE PENALTY
			if(vector.equalDirection(changeVector) || vector.equalDirection(changeVectorParrent) || changeVectorParrent.equalDirection(new PathVector(0,0,0)) ) {
				directionChangePenalty = 0;
			} else {
				//check for invalid u turns
				changeVectorParrent.invertDirection();
				if(vector.equalDirection(changeVectorParrent))
					return;
				//all other directions are peneltized
				directionChangePenalty = DIRECTION_CHANGE_COST_RAIL;
			}
			break;
		}
		
		//calculate G
		double newG = (currentTile.G + TILE_COST + Math.abs(currentTile.tileHight - processingTile.tileHight) * HEIGHT_COST  + directionChangePenalty);
		
		//is the tile not in the open list
		if (!(openTiles.contains(processingTile) || closedTiles.contains(processingTile))) {
			// put the tile in the open list
			openTiles.add(processingTile);
			//link it t its parent
			processingTile.setParent(currentTile);
			//calc G/H/F
			processingTile.G = newG;
			processingTile.H = calcManhattan(processingTile);
			processingTile.calcF();
		} else {
			//is the g value lower, change g and parent
			if (newG < processingTile.G) {
				
				processingTile.setParent(currentTile);
				processingTile.G = newG;
				//processingTile.H = calcManhattan(processingTile); // stays the same
				processingTile.calcF();
			}
		}
	}

	private Tile findMinF() {
		Tile minF = new Tile();
		minF.F = 9999;
		for(Tile tile : openTiles){
			if (tile.F < minF.F){
				minF = tile;
			}
		}
		return minF;
	}

	private double calcManhattan(Tile tile) {
		double x = Math.abs(tile.tileX - destination.tileX); 
		double y = Math.abs(tile.tileY - destination.tileY);
		return (x+y);
	}
}
